/*
 * @Author: Jiagang Chen
 * @Date: 2021-11-24 06:47:41
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-12-13 22:24:34
 * @Description: ...
 * @Reference: ...
 */

#include <unistd.h>
#include <iostream>
#include <string>

// YAML-CPP
#include <yaml-cpp/yaml.h>

#include "altimeter_converter.h"
#include "encoder_converter.h"
#include "fog_converter.h"
#include "gps_converter.h"
#include "imu_converter.h"
#include "sick_converter.h"
#include "stereo_converter.h"
#include "velodyne_converter.h"
#include "vrs_converter.h"

using namespace kaist_publisher;

enum SensorType {
    kAltimeter = 0,
    kEncoder,
    kFog,
    kGps,
    kVrs,
    kImu,
    kVelodyne,
    kStereo,
    kSensorTypeCount
};

std::vector<bool> sensors;
std::vector<std::string> sensor_names;
int publish_delay = -1;

AltimeterConverter::Ptr altimeter;
EncoderConverter::Ptr encoder;
FogConverter::Ptr fog;
GpsConverter::Ptr gps;
VrsConverter::Ptr vrs;
ImuConverter::Ptr imu;
VelodyneConverter::Ptr vlp;
StereoConverter::Ptr stereo;

std::string dataset = "";
std::string save_to = "";
std::string save_name = "urban";
std::string irp_topic = "/xsens_imu_data", raw_topic = "/imu/data_raw", mag_topic = "/imu/mag";
std::string stereo_left_topic = "/stereo/left/image_raw", stereo_right_topic = "/stereo/right/image_raw";
std::string vlp_left_topic = "/ns2/velodyne_points", vlp_right_topic = "/ns1/velodyne_points";
std::string vrs_topic = "/vrs_gps_data";
std::string gps_topic = "/gps/fix";
std::string altimeter_topic = "/altimeter_data";
std::string fog_topic = "/dsp1760_data";
std::string encoder_topic = "/encoder_count";

// std::queue<irp_sen_msgs::altimeterPtr> altimeterDataQueue;
// std::queue<irp_sen_msgs::encoderPtr> encoderDataQueue;
// std::queue<irp_sen_msgs::fog_3axisPtr> fogDataQueue;
// std::queue<sensor_msgs::NavSatFix::Ptr> gpsDataQueue;
// std::queue<irp_sen_msgs::vrsPtr> vrsDataQueue;
// std::queue<sensor_msgs::Imu::Ptr> imuDataQueue;
// std::queue<sensor_msgs::MagneticField::Ptr> magDataQueue;
// std::queue<sensor_msgs::PointCloud2::Ptr> leftLidarDataQueue;
// std::queue<sensor_msgs::PointCloud2::Ptr> rightLidarDataQueue;
// std::queue<sensor_msgs::Image::Ptr> leftStereoDataQueue;
// std::queue<sensor_msgs::Image::Ptr> rightStereoDataQueue;

// vilens::offlinePipeline::Ptr offline_pipeline_instance;

void loadKaistConfig(const std::string &config_path, ros::NodeHandle &nh);
// void pushDataStream(int cur_idx);

int count = 0;

int main(int argc, char **argv) {
    ros::init(argc, argv, "laserMapping");
    ros::NodeHandle nh;

    loadKaistConfig(std::string(argv[1]), nh);

    rosbag::Bag bag_out;
    bag_out.open(save_to + "/" + save_name + ".bag", rosbag::bagmode::Write);

    std::vector<int64_t> timestamp_sensor(sensors.size(), LONG_MAX);
    if (sensors[0])
        timestamp_sensor[0] = altimeter->Init_Publish();
    if (sensors[1])
        timestamp_sensor[1] = encoder->Init_Publish();
    if (sensors[2])
        timestamp_sensor[2] = fog->Init_Publish();
    if (sensors[3])
        timestamp_sensor[3] = gps->Init_Publish();
    if (sensors[4])
        timestamp_sensor[4] = vrs->Init_Publish();
    if (sensors[5])
        timestamp_sensor[5] = imu->Init_Publish();
    if (sensors[6])
        timestamp_sensor[6] = vlp->Init_Publish();
    if (sensors[7])
        timestamp_sensor[7] = stereo->Init_Publish();

    double sleep_time = (1000.0 / publish_delay);
    int cur_idx =
        int(min_element(timestamp_sensor.begin(), timestamp_sensor.end()) -
            timestamp_sensor.begin());
    while (timestamp_sensor[cur_idx] != LONG_MAX) {
        if (cur_idx == 0) {
            irp_sen_msgs::altimeterPtr data;
            altimeter->Publish(timestamp_sensor[cur_idx], data);
            bag_out.write(altimeter_topic.c_str(), data->header.stamp, data);
        } else if (cur_idx == 1) {
            irp_sen_msgs::encoderPtr data;
            encoder->Publish(timestamp_sensor[cur_idx], data);
            bag_out.write(encoder_topic.c_str(), data->header.stamp, data);
        } else if (cur_idx == 2) {
            irp_sen_msgs::fog_3axisPtr data;
            fog->Publish(timestamp_sensor[cur_idx], data);
            bag_out.write(fog_topic.c_str(), data->header.stamp, data);
        } else if (cur_idx == 3) {
            sensor_msgs::NavSatFix::Ptr data;
            gps->Publish(timestamp_sensor[cur_idx], data);
            bag_out.write(gps_topic.c_str(), data->header.stamp, data);
        } else if (cur_idx == 4) {
            irp_sen_msgs::vrsPtr data;
            vrs->Publish(timestamp_sensor[cur_idx], data);
            bag_out.write(vrs_topic.c_str(), data->header.stamp, data);
        } else if (cur_idx == 5) {
            sensor_msgs::Imu::Ptr data_imu;
            sensor_msgs::MagneticField::Ptr data_mag;
            imu->Publish(timestamp_sensor[cur_idx], data_imu, data_mag);
            bag_out.write(raw_topic.c_str(), data_imu->header.stamp, data_imu);
            bag_out.write(mag_topic.c_str(), data_mag->header.stamp, data_mag);
        } else if (cur_idx == 6) {
            sensor_msgs::PointCloud2::Ptr data_left;
            sensor_msgs::PointCloud2::Ptr data_right;
            vlp->Publish(timestamp_sensor[cur_idx], data_left, data_right);
            if (data_left)
                bag_out.write(vlp_left_topic.c_str(), data_left->header.stamp, data_left);
            if (data_right)
                bag_out.write(vlp_right_topic.c_str(), data_right->header.stamp, data_right);
        } else if (cur_idx == 7) {
            sensor_msgs::Image::Ptr data_left;
            sensor_msgs::Image::Ptr data_right;
            stereo->Publish(timestamp_sensor[cur_idx], data_left, data_right);
            bag_out.write(stereo_left_topic.c_str(), data_left->header.stamp, data_left);
            bag_out.write(stereo_right_topic.c_str(), data_right->header.stamp, data_right);
        }
        
        count++;
        if (count % 10000 == 0)
            std::cout << count << std::endl;
        // pushDataStream(cur_idx);
        // std::cout << cur_idx << std::endl;

        cur_idx =
            int(min_element(timestamp_sensor.begin(), timestamp_sensor.end()) -
                timestamp_sensor.begin());
        // usleep(sleep_time);
    }

    std::cout << "over" << std::endl;
    bag_out.close();
    std::cout << "over" << std::endl;

    ros::spin();
    return 0;
}

void loadKaistConfig(const std::string &config_path, ros::NodeHandle &nh) {
    auto config_node = YAML::LoadFile(config_path);
    if (!config_node) {
        std::exit(EXIT_FAILURE);
    }

    sensors.resize(kSensorTypeCount, false);
    sensor_names = {"altimeter", "encoder", "fog",      "gps",
                    "vrs",       "imu",     "velodyne", "stereo"};

    for (size_t i = 0; i < sensors.size(); ++i) {
        bool sensor_on = false;
        if (config_node[sensor_names[i]])
            sensor_on = config_node[sensor_names[i]].as<bool>();
        sensors[i] = sensor_on;
    }


    if (config_node["publish_delay"])
        publish_delay = config_node["publish_delay"].as<int>();
    publish_delay = publish_delay <= 0 ? 1 : publish_delay;

    
    if (config_node["dataset"])
        dataset = config_node["dataset"].as<std::string>();
    if (config_node["save_to"])
        save_to = config_node["save_to"].as<std::string>();
    if (config_node["save_name"])
        save_name = config_node["save_name"].as<std::string>();


    if (config_node["altimeter_topic"])
        altimeter_topic = config_node["altimeter_topic"].as<std::string>();
    altimeter.reset(
        new AltimeterConverter(nh, dataset, save_to, altimeter_topic));

    if (config_node["encoder_topic"])
        encoder_topic = config_node["encoder_topic"].as<std::string>();
    encoder.reset(new EncoderConverter(nh, dataset, save_to, encoder_topic));

    if (config_node["fog_topic"])
        fog_topic = config_node["fog_topic"].as<std::string>();
    fog.reset(new FogConverter(nh, dataset, save_to, fog_topic));

    if (config_node["gps_topic"])
        gps_topic = config_node["gps_topic"].as<std::string>();
    gps.reset(new GpsConverter(nh, dataset, save_to, gps_topic));

    if (config_node["vrs_topic"])
        vrs_topic = config_node["vrs_topic"].as<std::string>();
    vrs.reset(new VrsConverter(nh, dataset, save_to, vrs_topic));
    

    if (config_node["imu_irp_topic"])
        irp_topic = config_node["imu_irp_topic"].as<std::string>();
    if (config_node["imu_raw_topic"])
        raw_topic = config_node["imu_raw_topic"].as<std::string>();
    if (config_node["imu_mag_topic"])
        mag_topic = config_node["imu_mag_topic"].as<std::string>();
    imu.reset(new ImuConverter(nh, dataset, save_to, irp_topic, raw_topic,
                               mag_topic));

    if (config_node["velodyne_left_topic"])
        vlp_left_topic = config_node["velodyne_left_topic"].as<std::string>();
    if (config_node["velodyne_right_topic"])
        vlp_right_topic = config_node["velodyne_right_topic"].as<std::string>();
    vlp.reset(new VelodyneConverter(nh, dataset, save_to, vlp_left_topic,
                                    vlp_right_topic));


    if (config_node["stereo_left_topic"])
        stereo_left_topic = config_node["stereo_left_topic"].as<std::string>();
    if (config_node["stereo_right_topic"])
        stereo_right_topic =
            config_node["stereo_right_topic"].as<std::string>();
    stereo.reset(new StereoConverter(nh, dataset, save_to, stereo_left_topic,
                                     stereo_right_topic));
}

// void pushDataStream(int cur_idx) {
//     if (cur_idx == 0) {
//         return;  // altimeter
//     } else if (cur_idx == 1) {
//         return;  // encoder
//     } else if (cur_idx == 2) {
//         return;  // fog
//     } else if (cur_idx == 3) {
//         return;  // gps
//     } else if (cur_idx == 4) {
//         return;  // vrs
//     } else if (cur_idx == 5) {
//         if (!imuDataQueue.empty()) {
//             auto data_imu = imuDataQueue.front();
//             auto data_mag = magDataQueue.front();
//             imuDataQueue.pop();
//             magDataQueue.pop();

//             offline_pipeline_instance->pushData(data_imu);
//         }
//         return;
//     } else if (cur_idx == 6) {
//         if (!leftLidarDataQueue.empty()) {
//             auto data_left = leftLidarDataQueue.front();
//             auto data_right = rightLidarDataQueue.front();
//             leftLidarDataQueue.pop();
//             rightLidarDataQueue.pop();

//             if (data_left) {
//                 offline_pipeline_instance->pushData(data_left);
//             }
//         }
//         return;
//     } else if (cur_idx == 7) {
//         if (!leftStereoDataQueue.empty()) {
//             auto data_left = leftStereoDataQueue.front();
//             auto data_right = rightStereoDataQueue.front();
//             leftStereoDataQueue.pop();
//             rightStereoDataQueue.pop();

//             std::vector<std::pair<int, sensor_msgs::Image::Ptr>> image_data;
//             image_data.push_back(
//                 std::pair<int, sensor_msgs::Image::Ptr>(0, data_left));
//             image_data.push_back(
//                 std::pair<int, sensor_msgs::Image::Ptr>(1, data_right));
//             offline_pipeline_instance->pushData(image_data);
//         }
//         return;
//     }
//     return;
// }